package drone.iqgc.example;

import io.mavsdk.System;
import io.mavsdk.mission.Mission;
import io.mavsdk.mission.MissionProto.MissionPlan ;
import io.reactivex.CompletableSource;

import java.util.ArrayList;
import java.util.List;
import java.util.concurrent.CountDownLatch;
import java.util.concurrent.TimeUnit ;

import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

public class StartMission {
  private static final Logger logger = LoggerFactory.getLogger(StartMission.class);

  public static void main(String[] args) {
    logger.debug("Starting example: mission...");

    System drone = new System ( "60.205.58.145" , 50044 ) ;

    drone.getTelemetry().getFlightMode()
        .doOnNext(flightMode -> logger.debug("flight mode: " + flightMode))
        .subscribe();

    List<Mission.MissionItem> cycle = new ArrayList<>();
    cycle.add(generateMissionItem(47.398039859999997, 8.5455725400000002));
    cycle.add(generateMissionItem(47.398036222362471, 8.5450146439425509));
    cycle.add(generateMissionItem(47.397825620791885, 8.5450092830163271));
    cycle.add(generateMissionItem(47.397832880000003, 8.5455939999999995));

    List<Mission.MissionItem> missionItems = new ArrayList<>();
    for (int i = 0; i < 2; i++) {
      missionItems.addAll(cycle);
    }
    
    

    Mission.MissionPlan missionPlan = new Mission.MissionPlan(missionItems);
    logger.debug("About to upload " + missionItems.size() + " mission items");

    CountDownLatch latch = new CountDownLatch(1);
//    drone.getMission()
//        .setReturnToLaunchAfterMission(true)
//        .andThen(drone.getMission().uploadMission(missionPlan)
//            .doOnComplete(() -> logger.info("Upload succeeded")))
//        .andThen(drone.getMission().startMission()
//                .doOnSubscribe(disposable -> logger.info("start mission")).
//                doOnError ( err -> logger.info ( err.getMessage ( ) ) ).
//                doOnComplete ( () -> {logger.info ( "start complate" );} )
//                )
//        .delay ( 5 , TimeUnit.SECONDS )
//        .andThen ( drone.getMission ( ).pauseMission ( )
//        		.doOnSubscribe(disposable -> logger.info("pause mission")).
//                doOnError ( err -> logger.info ( err.getMessage ( ) ) ).
//                doOnComplete ( () -> {logger.info ( "pause complate" );} ) )
//        .andThen ( drone.getMission ( ).getMissionProgress ( )
//        		.doOnSubscribe(disposable -> logger.info("Progress mission")).
//                doOnError ( err -> logger.info ( err.getMessage ( ) ) ).
//                doOnComplete ( () -> {logger.info ( "Progress complate" );} )
//                )
//        .subscribe(per->{
//        	logger.info ( "sub comp : " +per);
//        });

    
  drone.getMission().startMission ( ).subscribe (()->{
	
	logger.info ("start mission");
} ,e -> e.printStackTrace ( ));
    
//    drone.getMission().isMissionFinished ( ).subscribe (result->{
//    	
//    	logger.info ( result.booleanValue ( ) +"");
//    } );
    
//    drone.getMission().getMissionProgress ( ).subscribe ( pro -> {
//    	
//    	logger.info ( pro.getCurrent ( )+"" );
//    	logger.info ( pro.getTotal ( )+"" );
//    	
//    } );
    

    
  
  }

  public static Mission.MissionItem generateMissionItem(double latitudeDeg, double longitudeDeg) {
	  
    return new Mission.MissionItem(
        latitudeDeg,
        longitudeDeg,
        10f,
        10f,
        true,
        Float.NaN,
        Float.NaN,
        Mission.MissionItem.CameraAction.NONE,
        Float.NaN,
        Double.NaN,
        Float.NaN,
        Float.NaN,
        Float.NaN);
  }
}

